/**
 * @file	Test_Beacon_Roomba.c
 * @author	Mitch, David
 * @brief
 */
#include "../header/common.h"
#include "../roomba/roomba.h"
#include "../roomba/roomba_sci.h"
#include "../radio/radio.h"
#include "../uart/uart.h"
#include <util/delay.h>

#ifdef TEST_BUMPER

#define     clock8MHz()    CLKPR = _BV(CLKPCE); CLKPR = 0x00;
const unsigned char PPP[] = {};
const unsigned int PT = sizeof(PPP)/2;
volatile uint8_t rxflag = 0;

int main(void) {

	clock8MHz();

	//initialize leds
	DDRD = (1<<PORTD4) | (1<<PORTD5) | (1<<PORTD6) | (1<<PORTD7);

	//initialize roomba
	Roomba_Init();

	//start driving left
	//roomba_Drive(100, 0x0001);

	uart_putchar(SENSORS);
	uart_putchar(1);
	while (uart_bytes_received() != 1);

	uint8_t bumps_wheeldrops = uart_get_byte(0);

	if (bumps_wheeldrops == 0) {
		PORTD ^= _BV(PD4);
	}
	else {
		PORTD ^= _BV(PD5);
	}

	return 0;
}

void radio_rxhandler(uint8_t pipenumber)
{
	rxflag = 1;
}

#endif
